#include <ESP32CAN.h>
#include <CAN_config.h>
#include<Arduino.h>

CAN_device_t CAN_cfg;               // CAN Config
unsigned long previousMillis = 0;   // will store last time a CAN Message was send
const int interval = 1000;          // interval at which send CAN Messages (milliseconds)
const int rx_queue_size = 10;       // Receive Queue size


void setup() {
  Serial.begin(115200);
  Serial.println("Basic Demo - ESP32-Arduino-CAN");
  CAN_cfg.speed = CAN_SPEED_125KBPS;
  CAN_cfg.tx_pin_id = GPIO_NUM_32;
  CAN_cfg.rx_pin_id = GPIO_NUM_33;
  CAN_cfg.rx_queue = xQueueCreate(rx_queue_size, sizeof(CAN_frame_t));
  // Init CAN Module
  ESP32Can.CANInit();
}

void loop() {

  CAN_frame_t rx_frame;

  unsigned long currentMillis = millis();

  // Receive next CAN frame from queue
  if (xQueueReceive(CAN_cfg.rx_queue, &rx_frame, 3 * portTICK_PERIOD_MS) == pdTRUE) {

    if (rx_frame.FIR.B.FF == CAN_frame_std) {
      printf("New standard frame");
    }
    else {
      printf("New extended frame");
    }

    if (rx_frame.FIR.B.RTR == CAN_RTR) {
      printf(" RTR from 0x%08X, DLC %d\r\n", rx_frame.MsgID,  rx_frame.FIR.B.DLC);
    }
    else {
      printf(" from 0x%08X, DLC %d, Data ", rx_frame.MsgID,  rx_frame.FIR.B.DLC);
      for (int i = 0; i < rx_frame.FIR.B.DLC; i++) {
        printf("0x%02X ", rx_frame.data.u8[i]);
      }
      printf("\n");
    }
  }
  // Send CAN Message
  if (currentMillis - previousMillis >= interval) {
    previousMillis = currentMillis;
    CAN_frame_t tx_frame;
    
    //Here defined std format so set not correct, Zhang Datou 42 motor use ext format !!! 
    //tx_frame.FIR.B.FF = CAN_frame_std;

    //set to ext format, ZY !
    tx_frame.FIR.B.FF = CAN_frame_ext;
    
    //ADDRESS 1 ,LEFT SHIFT 8 BITS FOR CAN CMD TRANSFER!
    tx_frame.MsgID = 0x100;
   
    //命令功能： 电机使能控制
    //命令格式： 地址 + 0xF3 + 0xAB + 使能状态 + 多机同步标志 + 校验字节
    //01 F3 AB 01 00 6B, CORRECT RESPONS: F3 02 6B
    // tx_frame.FIR.B.DLC = 5;
    // tx_frame.data.u8[0] = 0xF3;
    // tx_frame.data.u8[1] = 0xAB;
    // tx_frame.data.u8[2] = 0x01;
    // tx_frame.data.u8[3] = 0x00;
    // tx_frame.data.u8[4] = 0x6B;


  //  //命令功能： 速度模式控制，恒速运动，2,3 控制速度
  // //命令格式： 地址 + 0xF6 + 方向 + 速度 + 加速度 + 多机同步标志 + 校验字节
  // //命令示例： 发送 01 F6 01 05 DC 0A 00 6B， 命令正确返回 01 F6 02 6B
 
    // tx_frame.FIR.B.DLC = 7;
    // tx_frame.data.u8[0] = 0xF6;
    // tx_frame.data.u8[1] = 0x01;
    // tx_frame.data.u8[2] = 0x01;
    // tx_frame.data.u8[3] = 0xDC;
    // tx_frame.data.u8[4] = 0x0A;
    // tx_frame.data.u8[5] = 0x00;
    // tx_frame.data.u8[6] = 0x6B;



/*命令功能： 位置模式控制
命令格式： 地址 + 0xFD + 方向 + 速度+ 加速度 + 脉冲数 + 相对/绝对模式标志 +
多机同步标志 + 校验字节
命令返回： 地址 + 0xFD + 命令状态 + 校验字节
命令示例： 发送 01 FD 01 05 DC 00 00 00 7D 00 00 00 6B， 正确返回 01 FD 02 6B，
条件不满足返回 01 FD E2 6B， 错误命令返回 01 00 EE 

数据解析： 01 表示旋转方向为 CCW（ 00 表示 CW） ， 05 DC 表示速度为 0x05DC =
1500(RPM)， 00 表示加速度档位为 0x00 = 0， 00 00 7D 00 表示脉冲数为 0x00007D00 =
32000 个， 00 表示相对位置模式（01 表示绝对位置模式） ， 00 表示不启用多机同步（01
表示启用） ， 如果需要多个电机同步开始运动， 请参考“多机通讯及同步控制” 章节；
（16 细分下发送 3200 个脉冲电机旋转一圈， 32000 个脉冲表示转 10 圈, C80 转一圈

注意： 加速度档位为 0 表示不使用曲线加减速， 直接按照设定的速度运行。 曲线加
减速时间计算公式： t2 - t1 = (256 - acc) * 50(us)， Vt2 = Vt1 + 1(RPM)；
（注： acc 为加速度档位， Vt1为 t1 时刻的转速， ， Vt2 为 t2 时刻的转速
*/
    // > 8 bit data
    // SEND 1st Frame to motor 1
    // SEND DATA > 8, 1) CHANGE MSGID, 低八位加1 2) ADDING FUNC CODE BEFORE DATA
    tx_frame.MsgID = 0x100;
    tx_frame.FIR.B.DLC = 8;
    //DATA, data4 加速度
    tx_frame.data.u8[0] = 0xFD;
    tx_frame.data.u8[1] = 0x01;
    tx_frame.data.u8[2] = 0x05;
    tx_frame.data.u8[3] = 0xDC;
    tx_frame.data.u8[4] = 0x0F;
    tx_frame.data.u8[5] = 0x00;
    tx_frame.data.u8[6] = 0x00;
    tx_frame.data.u8[7] = 0x03;
    ESP32Can.CANWriteFrame(&tx_frame);
    // SEND 2ND FRAME
    tx_frame.MsgID = 0x101;
    tx_frame.FIR.B.DLC = 5;
    //DATA, 7, 1, Hex0380-> 1600 pwm, half circle.
    tx_frame.data.u8[0] = 0xFD;
    tx_frame.data.u8[1] = 0x80;
    tx_frame.data.u8[2] = 0x00;
    tx_frame.data.u8[3] = 0x00;
    tx_frame.data.u8[4] = 0x6B;
    ESP32Can.CANWriteFrame(&tx_frame);

    //--------------------------------Ｓｅｎｄ　Ｍｓｇ　ｔｏ　Ｍｏｔｏｒ　２　－－－－－－－－－－－－－－－－－
    // SEND 1st Frame to motor 2， address 0x20
    // SEND DATA > 8, 1) CHANGE MSGID, 低八位加1 2) ADDING FUNC CODE BEFORE DATA
    tx_frame.MsgID = 0x200;
    tx_frame.FIR.B.DLC = 8;
    //DATA, data4 加速度
    tx_frame.data.u8[0] = 0xFD;
    tx_frame.data.u8[1] = 0x01;
    tx_frame.data.u8[2] = 0x05;
    tx_frame.data.u8[3] = 0xDC;
    tx_frame.data.u8[4] = 0x0F;
    tx_frame.data.u8[5] = 0x00;
    tx_frame.data.u8[6] = 0x00;
    tx_frame.data.u8[7] = 0x03;
    ESP32Can.CANWriteFrame(&tx_frame);
    // SEND 2ND FRAME
    tx_frame.MsgID = 0x201;
    tx_frame.FIR.B.DLC = 5;
    //DATA, 7, 1, Hex0380-> 1600 pwm, half circle.
    tx_frame.data.u8[0] = 0xFD;
    tx_frame.data.u8[1] = 0x80;
    tx_frame.data.u8[2] = 0x00;
    tx_frame.data.u8[3] = 0x00;
    tx_frame.data.u8[4] = 0x6B;
    ESP32Can.CANWriteFrame(&tx_frame);


  // Get Motor info
  //  tx_frame.FIR.B.DLC = 2;
  //   tx_frame.data.u8[0] = 0x20;
  //   tx_frame.data.u8[1] = 0x6B;






  ESP32Can.CANWriteFrame(&tx_frame);

  delay(2000);

  }
}
